import numpy as np
from traci import vehicle
from .Message import Task
from .ComputingUnit import *
import heapq
import math
from simulation_constants import *


class Scenario:
    # 时间维度
    frame = 0
    
    traffic_scenario_root = None    # 本次场景对应的交通场景根字典
    obus_policy_dict = None  # 场景中当前存在的所有OBU策略

    # 场景中的计算单元实体, 使用字典存储, item 为 {id: CU}
    vehicles = dict()                                                             # 场景中当前存在的所有车辆
    rsus = dict()                                                                 # 场景中当前存在的所有边缘服务单元(RSU)
    wait_for_rsu = RSU(id="wait_for_rsu", x=10000000, y=10000000, computing_power=0.0)      # 初始RSU, 刚进入场景的车辆会被分配此RSU

    # 本场景中存在的无线信道传输占用, 用于计算当前场景无线信道SINR中的I干扰, 使用 优先队列 实现过期占用清除
    wireless_channel_heap = []           # 每个元素的类型为列表 [传输终止帧序号, 处理后传输功率]
    wireless_channel_SINR_I = 0          # 当前场景无线信道SINR中的I干扰
    last_print_frame = -1         # 上一次打印的时间帧（用于节流）

    def __init__(self, rsus):
        """ 场景初始化函数, 仅初始化固定的RSU
        :param rsus: 场景中存在的所有RSU, 类型为字典, item 为 {id: RSU}
        """
        self.rsus = rsus


    def step(self, step_i: int) -> list:
        
        """
        场景更新函数, 用于更新场景中所有实体的状态
        :param step_i: 当前场景时间步
        :return: 当前场景时间步中产生的任务队列
        """
        # 更新场景时间
        self.frame = step_i
        # sum_vehs += int(root[frame].attrib["num_of_vehicles"])
        # 初始化当前时隙的任务队列
        task_queue = list()
        # 遍历该时刻场景中的所有车辆
        for vehicle in self.traffic_scenario_root[step_i]:
            if vehicle.attrib["id"] in self.vehicles:
                # 该车辆之前就在场景中, 更新其位置和速度
                self.vehicles[vehicle.attrib["id"]].x = float(vehicle.attrib["x"])
                self.vehicles[vehicle.attrib["id"]].y = float(vehicle.attrib["y"])
                self.vehicles[vehicle.attrib["id"]].velocity = float(vehicle.attrib["speed"])
                self.vehicles[vehicle.attrib["id"]].angle = float(vehicle.attrib["angle"])
            else:
                # 该车辆刚刚进入场景, 将该车辆加入场景中
                self.vehicles[vehicle.attrib["id"]] = VehicleOrOBU(id=vehicle.attrib["id"],
                                                                        x=float(vehicle.attrib["x"]),
                                                                        y=float(vehicle.attrib["y"]),
                                                                        velocity=float(vehicle.attrib["speed"]),
                                                                        enter_scenario_time=int(step_i),
                                                                        angle=float(vehicle.attrib["angle"]),
                                                                        os_policy=self.obus_policy_dict.get(vehicle.attrib["id"], None),
                                                                        manager_RSU=self.wait_for_rsu)
            # 设置更新标志
            self.vehicles[vehicle.attrib["id"]].update_flag = True
            # 将当前时隙当前车辆产生的任务添加到当前时隙的任务队列中
            for task in vehicle:
                task_queue.append(Task(task_vehicle_id=vehicle.attrib["id"],
                                        data_size=float(task.attrib["data_size"]),
                                        computing_power_demand=float(task.attrib["computing_power_demand"]),
                                        type=int(task.attrib["type"]),
                                        maximum_allowable_delay=float(task.attrib["maximum_allowable_delay"])))

        # 根据时间帧序号清理已经过期的信息(驶出场景的车辆, 传输完成的信号, 计算完毕的任务)
        self.update_scenario()
        return task_queue


    def calculate_task_offloading_result(self,
                                      task: Task,
                                      task_vehicle: VehicleOrOBU,
                                      offloading_decision: list) -> float:
        """ 
        计算任务卸载结果并返回奖励
        :param task: 待卸载的任务
        :param taskVehicle: 任务所属的车辆
        :param offloading_decision: 所有具有优势值的CU的卸载决策, 元素为 (CU, 卸载比例, 任务占用计算资源量)
        :return reward: 任务卸载的奖励值
        :return total_time: 任务卸载的总耗时, -1 表示卸载失败(通信维持时间 < totalTime)
        :return is_timeout: 任务是否超时
        :return total_energy: 任务处理能耗值, 单位为 J
        """
        # 逐次调用的诊断打印已移除，业务逻辑保持不变

        # 完成本计算任务的总用时, 卸载失败为 -1
        total_time = -1

        # 任务是否超时, 超时为 True, 默认为 False
        is_timeout = False

        # 任务处理能耗值, 单位为 J
        total_energy = 0

        # 遍历具有优势值的所有CU的卸载决策
        for oD in offloading_decision:
            # 如果对此CU的任务卸载比例不为 0, 即有部分或全部任务需要卸载到此 CU, 则开始进行计算
            if oD[1] != 0:
                # 本地计算
                if task_vehicle.id == oD[0].id:
                    # 如果本地计算任务车辆的下一空闲时间晚于任务最大允许延迟, 则直接返回卸载失败, 不放入任务队列进行计算
                    if self.vehicles[oD[0].id].next_idle_time_frame > self.frame + task.maximum_allowable_delay / 100:
                        return 0, -1, True, 0

                    # 计算耗时(单位为秒)
                    computation_time = task.computing_power_demand * oD[1] / (
                                self.vehicles[oD[0].id].computing_power * (10 ** 3))

                    # 计算服务CU下一空闲时间(单位为帧序号, 0.1 秒为一帧)
                    self.vehicles[oD[0].id].next_idle_time_frame = max(self.frame, self.vehicles[
                        oD[0].id].next_idle_time_frame) + computation_time * 10

                    # 还原为总的处理时间(单位为秒)
                    total_time = (self.vehicles[oD[0].id].next_idle_time_frame - self.frame) / 10
                    
                    # 本地处理的部分超时, 返回没有奖励
                    if total_time > task.maximum_allowable_delay / 1000:
                        is_timeout = True
                    
                    # 计算本地处理计算能耗
                    total_energy = Scenario.calculate_computation_energy(self.vehicles[oD[0].id].computing_power, task.computing_power_demand * oD[1])
                else:
                    # 卸载到 服务车辆/所属RSU
                    if self.actual_wireless_channel_connection_time(oD[0], task_vehicle) == -1:
                        # 如果处于无线信号连接范围外, 直接返回卸载失败
                        return 0, -1, True, 0

                    # 计算 数据发送功率 和 数据传输速率（考虑11bd的移动性与MAC竞争影响）
                    distance = math.sqrt(abs(task_vehicle.x - oD[0].x) ** 2 + abs(task_vehicle.y - oD[0].y) ** 2)

                    # 近似相对速度：车辆与服务CU之间的相对速度大小
                    if type(oD[0]) is VehicleOrOBU:
                        v1x = oD[0].velocity * math.sin(math.radians(oD[0].angle))
                        v1y = oD[0].velocity * math.cos(math.radians(oD[0].angle))
                        v2x = task_vehicle.velocity * math.sin(math.radians(task_vehicle.angle))
                        v2y = task_vehicle.velocity * math.cos(math.radians(task_vehicle.angle))
                        relative_speed = math.sqrt((v2x - v1x) ** 2 + (v2y - v1y) ** 2)
                    else:
                        # RSU 静止：相对速度为任务车辆速度
                        relative_speed = task_vehicle.velocity

                    # 当前活跃链路数量（优先队列内尚未结束的传输）
                    now_active_links = sum(1 for end_frame, _ in self.wireless_channel_heap if end_frame > self.frame)

                    transmission_power, transfer_rate = Scenario.wireless_channel_data_transmission_rate(
                        distance, relative_speed=relative_speed, n_active_links=now_active_links)

                    # 计算数据传输时间
                    transmission_time = task.data_size * oD[1] / transfer_rate

                    # 更新本场景无线信道状态
                    heapq.heappush(self.wireless_channel_heap, (self.frame + transmission_time * 10, transmission_power))

                    self.wireless_channel_SINR_I += transmission_power

                    if type(oD[0]) is VehicleOrOBU:
                        # 如果服务CU的下一空闲时间晚于任务最大允许延迟, 则直接返回卸载失败, 不放入任务队列进行计算
                        if self.vehicles[oD[0].id].next_idle_time_frame > self.frame + transmission_time * 10 + task.maximum_allowable_delay / 100:
                            return 0, -1, True, 0
                    else:
                        # 如果RSU的下一空闲时间晚于任务最大允许延迟, 则直接返回卸载失败, 不放入任务队列进行计算
                        if self.rsus[oD[0].id].next_idle_time_frame > self.frame + transmission_time * 10 + task.maximum_allowable_delay / 100:
                            return 0, -1, True, 0

                    if type(oD[0]) is VehicleOrOBU:
                        computation_time = task.computing_power_demand * oD[1] / (
                                    self.vehicles[oD[0].id].computing_power * (10 ** 3))
                    else:
                        computation_time = task.computing_power_demand * oD[1] / (
                                    self.rsus[oD[0].id].computing_power * (10 ** 3))

                    if type(oD[0]) is VehicleOrOBU:
                        nITF = max(self.frame + transmission_time * 10,
                                   self.vehicles[oD[0].id].next_idle_time_frame) + computation_time * 10
                        self.vehicles[oD[0].id].next_idle_time_frame = nITF
                    else:
                        nITF = max(self.frame + transmission_time * 10,
                                   self.rsus[oD[0].id].next_idle_time_frame) + computation_time * 10
                        self.rsus[oD[0].id].next_idle_time_frame = nITF

                    total_time = (nITF - self.frame) / 10

                    if type(oD[0]) is VehicleOrOBU:
                        total_energy = Scenario.calculate_computation_energy(self.vehicles[oD[0].id].computing_power, task.computing_power_demand * oD[1]) + Scenario.calculate_communication_energy(transmission_time)
                        # print(f"传输能耗{Scenario.calculate_communication_energy(transmission_time)},计算能耗{Scenario.calculate_computation_energy(self.vehicles[oD[0].id].computing_power, task.computing_power_demand * oD[1])}")
                    else:
                        # 卸载到 RSU 的计算任务能耗只计算传输能耗, 忽略计算能耗
                        # total_energy = Scenario.calculate_computation_energy(self.rsus[oD[0].id].computing_power, task.computing_power_demand * oD[1]) + Scenario.calculate_communication_energy(transmission_time)
                        total_energy = Scenario.calculate_communication_energy(transmission_time)
                    
                    if total_time > self.actual_wireless_channel_connection_time(oD[0], task_vehicle):
                        if DEBUG:
                            print(f"任务处理失败, 任务处理总耗时{total_time}秒, 超过信道维持时间{self.actual_wireless_channel_connection_time(oD[0], task_vehicle)}秒")
                        return 0, -1, True, total_energy

                    if total_time > task.maximum_allowable_delay / 1000:

                        is_timeout = True
        if DEBUG:
            if is_timeout:
                print(f"任务处理成功但超时, 任务处理总耗时{total_time}秒, 超过最大允许延迟{task.maximum_allowable_delay/1000}秒")                        
            else:
                print(f"任务处理成功, 任务处理总耗时{total_time}秒, 未超过最大允许延迟{task.maximum_allowable_delay/1000}秒")
        # 全部任务卸载成功, 返回奖励函数计算的奖励值, 总耗时, 是否超时, 能耗值
        # print(f"任务奖励值{Scenario.reward_fun(total_time, task)} + 能耗值{total_energy}")
        return Scenario.reward_fun(total_time, task), total_time, is_timeout, total_energy


    def calculate_computation_energy(computing_power: float, computing_power_demand: float) -> float:
        """
        计算计算能耗
        :param computing_power: 计算能力, 单位为 G Cycle/s
        :param computing_power_demand: 任务计算能力需求, 单位为 M Cycles
        :return: 计算能耗值, 单位为 J
        """
        computation_energy = KAPPA * (computing_power * 10 ** 9) ** 2 * (computing_power_demand * 10 ** 6)

        return computation_energy


    def calculate_communication_energy(communication_time: float) -> float:
        """
        计算通信能耗
        :param communication_time: 通信时间, 单位为秒
        :return: 通信能耗值, 单位为 J
        """
        communication_energy = P * communication_time

        return communication_energy


    def reward_fun(total_time, task) -> float:
        """ 任务卸载奖励函数
        :param totalTime: 任务处理总耗时(单位为秒)
        :param task: 任务
        :return: 奖励值
        """
        # reward = Scenario.omega_4 * task.type * (task.maximum_allowable_delay - (totalTime * 1000)) / task.maximum_allowable_delay * 500
        reward = omega_4 * task.type * (task.maximum_allowable_delay - (total_time * 1000))
        return reward


    def select_RSU(self, task_vehicle):
        """计算该车辆所属RSU, 即距离最近的RSU
        :param taskVehicle: 任务车辆
        :return: 距离最近的RSU
        """
        min_distance = 100000000000
        for rsu in self.rsus.values():
            distance = (rsu.x - task_vehicle.x) ** 2 + (rsu.y - task_vehicle.y) ** 2
            if distance < min_distance:
                min_distance = distance
                candidate_RSU = rsu
        return candidate_RSU


    def select_advantageous_vehicles(self, task_vehicle) -> list:
        """ 从场景中除任务车辆外的计算单元中选出具有较大相对优势值的服务车辆、RSU及车辆自身, 其中RSU固定为最接近服务车辆的RSU
        :param taskVehicle: 任务车辆
        :return: 包含优势车辆的列表, 列表元素为 [所属RSU, 优势车辆1, 优势车辆2, 优势车辆3, 优势车辆4, 车辆自身]
        """

        def normalization_preparation(task_vehicle: VehicleOrOBU) -> (list, dict):
            """ 为优势值计算函数中的数值归一化做准备工作
            :param taskVehicle: 任务车辆
            :return: 包含归一化参数的列表, 列表元素为 [[影响因子1极值差, 影响因子1最小值], [影响因子2极值差, 影响因子2最小值], [影响因子3极值差, 影响因子3最小值]]
            """
            # 废弃影响因子: 车辆距离, 该影响因子与 估计无线信道连接时间 有很大正相关性, 因此从精简影响因子的角度废弃该影响因子
            # dis_min = 0
            # dis_max = 0
            # 影响因子1: 估计无线信道连接时间
            cT_min = 0
            cT_max = 0
            # 影响因子2: 车辆计算能力
            rCP_min = 0
            rCP_max = 0
            # 影响因子3: 车辆剩余忙碌时间, 计算方式为 (vehicle.nextIdleTimeFrame - frame), 此值 <= 0 表示当前空闲
            nIT_min = 0
            nIT_max = 0
            other_vehicles = dict()
            for vehicle in self.vehicles.values():
                if vehicle.id != task_vehicle.id:
                    # dis = pow((taskVehicle.x - vehicle.x) ** 2 + (taskVehicle.y - vehicle.y) ** 2, 0.5)
                    # dis_min = min(dis, dis_min)
                    # dis_max = max(dis, dis_max)
                    connection_time = self.predicted_wireless_channel_connection_time(vehicle, task_vehicle)
                    cT_min = min(connection_time, cT_min)
                    cT_max = max(connection_time, cT_max)
                    rCP_min = min(rCP_min, vehicle.computing_power)
                    rCP_max = max(rCP_max, vehicle.computing_power)
                    nIT = max(vehicle.next_idle_time_frame - self.frame, 0)
                    nIT_min = min(nIT_min, nIT)
                    nIT_max = max(nIT_max, nIT)
                    other_vehicles[vehicle.id] = (connection_time, vehicle.computing_power, nIT)
            return (
            [[cT_max - cT_min, cT_min], [rCP_max - rCP_min, rCP_min], [nIT_max - nIT_min, nIT_min]], other_vehicles)

        def calculate_advantage_value(target_CU: CU, nPs: list) -> float:
            """ 计算任务车辆与目标卸载单元之间的优势值, 对三个影响因子都进行归一化处理
            :param targetCU: 目标卸载单元
            :param nPs: 归一化参数列表, 列表元素为 [[影响因子1极值差, 影响因子1最小值], [影响因子2极值差, 影响因子2最小值], [影响因子3极值差, 影响因子3最小值]]
            :return: 优势值
            """
            # 获取两个车辆之间的距离, 并进行归一化
            # distance = nPs[1][targetCU.id][0]
            # distance = (distance - nPs[0][0][1]) / nPs[0][0][0]
            # 获取两个车辆之间估算的通信维持时间, 并进行归一化
            connection_time = (nPs[1][target_CU.id][0] - nPs[0][0][1]) / (nPs[0][0][0] + non_zero)
            # 获取目标车辆的计算能力, 并进行归一化
            remaining_computing_power = (nPs[1][target_CU.id][1] - nPs[0][1][1]) / (nPs[0][1][0] + non_zero)
            # 获取车辆剩余忙碌时间, 并进行归一化
            busy_time = (nPs[1][target_CU.id][2] - nPs[0][2][1]) / (nPs[0][2][0] + non_zero)

            # 加权返回
            return omega_1 * connection_time + omega_2 * remaining_computing_power + omega_3 * busy_time

        # 返回结果的列表
        ACUs = list()
        # 计算距离最近的RSU

        candidate_RSU = self.select_RSU(task_vehicle)

        ACUs.append(candidate_RSU)

        # 为计算相对优势值的归一化进行参数准备
        normalization_parameters = normalization_preparation(task_vehicle)

        # 计算最具有相对优势值的4个服务车辆
        candidate_OBUs = []
        for vehicle in self.vehicles.values():
            if vehicle.id != task_vehicle.id:
                candidate_OBUs.append((calculate_advantage_value(vehicle, normalization_parameters), vehicle))

        # 根据相对优势值排序, 优势值越大越靠前
        candidate_OBUs.sort(key=lambda x: -x[0])

        # 选出优势值前4名
        for _ in range(4):
            ACUs.append(candidate_OBUs[_][1])

        ACUs.append(task_vehicle)

        return ACUs


    def wireless_channel_data_transmission_rate(d: float, relative_speed: float = 0.0, n_active_links: int = None) -> (float, float):
        """ 计算无线信道中的数据传输速率函数, 返回值为元组, 两个元素表示为 (等效发送功率 W, 数据传输速率 MBps)
        :param d: 车辆与卸载单元之间的距离 (米)
        :param relative_speed: 两节点相对速度 (m/s)，用于近似多普勒影响（可选）
        :param n_active_links: 当前活跃链路数，用于近似 MAC 层竞争影响（可选）
        :return: 元组, 第一个元素为等效发送功率 W, 第二个元素为应用层有效数据传输速率 MBps
        """
        # 近似接收功率（路径损耗模型）
        d_eff = max(d, 1.0)
        pr = P * h * (d_eff ** -alpha)

        # 线性 SINR 与 dB
        sinr_linear = pr / (N + Scenario.wireless_channel_SINR_I + non_zero)
        sinr_db = 10.0 * np.log10(max(sinr_linear, non_zero))

        # 11bd：考虑移动性惩罚与处理增益
        if BD_ENABLED:
            mobility_penalty_db = min(6.0, (relative_speed / 30.0) * 2.0)
            effective_snr_db = sinr_db - mobility_penalty_db + BD_PROCESSING_GAIN_DB
        else:
            effective_snr_db = sinr_db

        # 简化 MCS 表（阈值 -> 光谱效率 bps/Hz），适度放宽阈值以提升速率（-2dB）
        mcs_table = [
            (1.0, 1.0),    # QPSK 1/2（原3.0）
            (4.0, 2.0),    # 16-QAM 1/2（原6.0）
            (8.0, 3.0),    # 16-QAM 3/4（原10.0）
            (14.0, 4.0),   # 64-QAM 2/3（原16.0）
            (18.0, 4.5),   # 64-QAM 3/4（原20.0）
            (24.0, 6.0),   # 256-QAM 3/4（原26.0）
            (28.0, 6.66)   # 256-QAM 5/6（原30.0）
        ]
        spectral_efficiency = 0.5
        for thr_db, se in mcs_table:
            if effective_snr_db >= thr_db:
                spectral_efficiency = se

        # 物理层速率（bps）
        r_phy_bps = W * 1e6 * spectral_efficiency

        # MAC 层净吞吐效率：考虑活跃链路导致的竞争/退化（经验近似）
        if n_active_links is None:
            now_active = sum(1 for end_frame, _ in Scenario.wireless_channel_heap if end_frame > Scenario.frame)
        else:
            now_active = n_active_links
        x = max(0, now_active - 1)
        if MAC_BACKOFF_MODEL_ENABLED:
            # 非线性退化拟合：Padé 型有理函数，体现碰撞概率与退避成本的非线性增长
            mac_eff = BD_MAC_OVERHEAD_BASE / (1.0 + MAC_BACKOFF_BETA * x + MAC_BACKOFF_GAMMA * x * x)
        else:
            # 线性退化（原实现，保持向后兼容）
            mac_eff = BD_MAC_OVERHEAD_BASE - 0.02 * x
        # 夹紧到安全区间
        mac_eff = max(MAC_BACKOFF_MIN, min(MAC_BACKOFF_MAX, mac_eff))

        r_net_bps = r_phy_bps * mac_eff
        transferRate_MBps = r_net_bps / (8.0 * 1e6)

        # 调试打印（每帧只打印一次，以避免日志过载）
        if BD_DEBUG_PRINT and last_print_frame != frame:
            bd_max_dis = bd_phy_max_range()
            print(
                f"[11bd通信层] 帧:{Scenario.frame} 距离:{d:.2f}m 相对速度:{relative_speed:.2f}m/s 活跃链路:{now_active} | "
                f"Pr:{pr:.3e}W SINR:{sinr_db:.2f}dB 有效SNR:{effective_snr_db:.2f}dB SE:{spectral_efficiency:.2f}bps/Hz | "
                f"PHY速率:{r_phy_bps/1e6:.2f}Mbps MAC效率:{mac_eff:.2f} 有效净速率:{transferRate_MBps:.3f}MB/s | "
                f"当前估计最大通信范围:{bd_max_dis:.2f}m"
            )
            Scenario.last_print_frame = Scenario.frame

        return float(pr), float(transferRate_MBps)

    @staticmethod
    def bd_phy_max_range() -> float:
        """在最低 MCS 工作阈值下估计的最大通信距离（米）。"""
        if not BD_ENABLED:
            return MAX_CONNECT_DISTANCE
        thr_db = BD_MIN_MCS_SNR_DB
        thr_linear = 10 ** (thr_db / 10.0)
        required_pr = thr_linear * (N + Scenario.wireless_channel_SINR_I + non_zero)
        base = (P * h) / max(required_pr, non_zero)
        d = max(1.0, base ** (1.0 / alpha))
        d *= 2.0  # 进一步提升覆盖能力（在保证保守上限的前提下）
        return float(min(d, MAX_CONNECT_DISTANCE * 3.0))


    def predicted_wireless_channel_connection_time(self, service_cu, task_cu, max_connection_dis=MAX_CONNECT_DISTANCE) -> float:
        """ 估算两个计算单元CU之间的无线信道维持时间, cu1 为 服务CU, cu2 为任务CU
        :param cu1 (VehicleOrOBU): 服务CU
        :param cu2 (VehicleOrOBU): 任务CU
        :return (float): 无线信道维持时间, 单位为0.1秒, 即仿真间隔
        """

        def preview(v_x, v_y, x, y, t):
            x += v_x * t
            y += v_y * t
            return math.sqrt(x ** 2 + y ** 2)

        # 计算两个车辆初始距离
        dis_begin = math.sqrt(abs(service_cu.x - task_cu.x) ** 2 + abs(service_cu.y - task_cu.y) ** 2)
        # 11bd：动态最大通信距离（覆盖能力提升）
        bd_max_dis = Scenario.bd_phy_max_range()
        max_dis = max_connection_dis if not BD_ENABLED else max(max_connection_dis, bd_max_dis)
        # 大于最大通信距离则直接返回 -1, 表示无法连接
        if dis_begin > max_dis:
            return -1
        # 转换cu1, cu2 的绝对坐标为 cu2 相对于 cu1 的相对坐标 cu_loc_re
        cu_loc_re = (task_cu.x - service_cu.x, task_cu.y - service_cu.y)
        # 转换cu1, cu2 的绝对速度为 cu2 相对于 cu1 的相对速度 cu_vec_re, 如果cu1 为RSU则不需经过此转换
        if type(service_cu) is VehicleOrOBU:
            cu1_x_vec = service_cu.velocity * math.sin(math.radians(service_cu.angle))
            cu1_y_vec = service_cu.velocity * math.cos(math.radians(service_cu.angle))
            cu2_x_vec = task_cu.velocity * math.sin(math.radians(task_cu.angle))
            cu2_y_vec = task_cu.velocity * math.cos(math.radians(task_cu.angle))
            # x, y 方向分量合并
            cu2_x_vec -= cu1_x_vec
            cu2_y_vec -= cu1_y_vec
        else:
            cu2_x_vec = task_cu.velocity * math.sin(math.radians(task_cu.angle))
            cu2_y_vec = task_cu.velocity * math.cos(math.radians(task_cu.angle))
        
        left = MIN_PREDICTED_CONNECTION_TIME_INTERVAL
        right = MAX_PREDICTED_CONNECTION_TIME

        if preview(cu2_x_vec, cu2_y_vec, cu_loc_re[0], cu_loc_re[1], MAX_PREDICTED_CONNECTION_TIME) < MAX_PREDICTED_CONNECTION_TIME:
            return MAX_PREDICTED_CONNECTION_TIME
        if preview(cu2_x_vec, cu2_y_vec, cu_loc_re[0], cu_loc_re[1], MIN_PREDICTED_CONNECTION_TIME_INTERVAL) > MAX_PREDICTED_CONNECTION_TIME:
            return MIN_PREDICTED_CONNECTION_TIME_INTERVAL

        while left <= right:
            mid = (right - left) / 2 + left
            dis = preview(cu2_x_vec, cu2_y_vec, cu_loc_re[0], cu_loc_re[1], mid)
            if abs(MAX_PREDICTED_CONNECTION_TIME - dis) < MIN_PREDICTED_CONNECTION_TIME_INTERVAL:
                return mid
            elif dis > MAX_PREDICTED_CONNECTION_TIME:
                right = mid
            else:
                left = mid
        return -1


    def actual_wireless_channel_connection_time(self, service_cu, task_cu, max_connection_dis=MAX_CONNECT_DISTANCE) -> float:
        """ 通过对于未来交通场景进行搜索以获取两个计算单元CU之间的实际的无线信道维持时间, cu1 为 服务CU, cu2 为任务CU
        :param cu1 (CU): 服务CU
        :param cu2 (VehicleOrOBU): 任务CU
        :return (float): 无线信道维持时间, 单位为0.1秒, 即仿真间隔
        """
        task_cu_x = task_cu.x
        task_cu_y = task_cu.y
        task_cu_angle = task_cu.angle
        task_cu_velocity = task_cu.velocity
        service_cu_x = service_cu.x
        service_cu_y = service_cu.y
        
        if type(service_cu) is VehicleOrOBU:
            service_cu_angle = service_cu.angle
            service_cu_velocity = service_cu.velocity
        actual_connection_time = -1
        frame = self.frame

        # 11bd：动态最大通信距离（覆盖能力提升）
        bd_max_dis = Scenario.bd_phy_max_range()
        max_dis = max_connection_dis if not BD_ENABLED else max(max_connection_dis, bd_max_dis)
        while (math.sqrt((task_cu_x - service_cu_x) ** 2 + (task_cu_y - service_cu_y) ** 2)) <= max_dis:

            frame += 1
            actual_connection_time = (frame - self.frame) / 10
            
            if actual_connection_time >= MAX_PREDICTED_CONNECTION_TIME:
                # 避免长时间无意义运算, 最长连接时间就为 20 秒
                return MAX_PREDICTED_CONNECTION_TIME

            if frame >= len(self.traffic_scenario_root) or task_cu.id not in self.traffic_scenario_root[frame]:
                # 已知当前位置x、y，角度angle（度），速度velocity（m/s），求0.1秒后的新位置
                delta_t = 0.1  # 时间步长 0.1 秒
                # 将角度转换为弧度，注意：angle 为车辆朝向与正北方向（y轴正方向）的夹角，顺时针为正
                rad = math.radians(task_cu_angle)
                # 计算位移分量
                dx = task_cu_velocity * math.sin(rad) * delta_t
                dy = task_cu_velocity * math.cos(rad) * delta_t
                # 更新位置
                task_cu_x += dx
                task_cu_y += dy
            else:
                task_cu_x = self.traffic_scenario_root[frame][task_cu.id].x
                task_cu_y = self.traffic_scenario_root[frame][task_cu.id].y
                task_cu_angle = self.traffic_scenario_root[frame][task_cu.id].angle
                task_cu_velocity = self.traffic_scenario_root[frame][task_cu.id].velocity                

            if type(service_cu) is RSU:
                continue

            if frame >= len(self.traffic_scenario_root) or service_cu.id not in self.traffic_scenario_root[frame]:
                # 已知当前位置x、y，角度angle（度），速度velocity（m/s），求0.1秒后的新位置
                delta_t = 0.1  # 时间步长 0.1 秒
                # 将角度转换为弧度，注意：angle 为车辆朝向与正北方向（y轴正方向）的夹角，顺时针为正
                rad = math.radians(service_cu_angle)
                # 计算位移分量
                dx = service_cu_velocity * math.sin(rad) * delta_t
                dy = service_cu_velocity * math.cos(rad) * delta_t
                # 更新位置
                service_cu_x += dx
                service_cu_y += dy
            else: 
                service_cu_x = self.traffic_scenario_root[frame][service_cu.id].x
                service_cu_y = self.traffic_scenario_root[frame][service_cu.id].y
                service_cu_angle = self.traffic_scenario_root[frame][service_cu.id].angle
                service_cu_velocity = self.traffic_scenario_root[frame][service_cu.id].velocity

        return actual_connection_time

    
    def clean_vehiclesOutside(self):
        """根据更新标志清理已经驶出测试场景的车辆"""
        remove_keys = set()
        for vehicle_key in self.vehicles.keys():
            if self.vehicles[vehicle_key].update_flag:
                self.vehicles[vehicle_key].update_flag = False
            else:
                remove_keys.add(vehicle_key)
        # for rk in remove_keys:
        #     print(f"{rk} 在场景中停留了{(self.frame - self.vehicles[rk].enterScenarioTime)/10}秒")
        #     self.vehicles.pop(rk)

    
    def clean_CU_taskQueue(self, frame: int):
        # (废弃) 清理 CU 中已完成的任务, 释放被占用的计算资源
        pass


    def clean_wirelessChannel(self):
        """ 清除无线信道中的过期传输数据 """
        while len(self.wireless_channel_heap) > 0 and self.wireless_channel_heap[0][0] <= self.frame:
            self.wireless_channel_SINR_I -= heapq.heappop(self.wireless_channel_heap)[1]


    def update_scenario(self):
        """根据时间帧序号清理已经过期的信息(驶出场景的车辆, 传输完成的信号, 计算完毕的任务)"""
        self.clean_wirelessChannel()
        self.clean_vehiclesOutside()
        # self.Clean_CU_TaskQueue(frame)


    def reset_scenario(self, traffic_scenario_root: dict, obus_policy_dict: dict):
        """ 场景重置 """
        self.traffic_scenario_root = traffic_scenario_root
        self.obus_policy_dict = obus_policy_dict
        for rsu in self.rsus.values():
            rsu.next_idle_time_frame = 0
        self.vehicles.clear()
